#include <Arduino.h>
#include <TinyGPSPlus.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <SPIFFS.h>
#include <Wire.h>
#include <MPU6050_tockn.h>

// 启用硬件浮点运算优化
#pragma GCC optimize ("O3")

// 硬件连接定义
#define GPS_RX 16
#define GPS_TX 17
#define OLED_SDA 21
#define OLED_SCL 20
#define MPU6050_ADDR 0x68  // MPU6050默认地址

// OLED屏幕尺寸
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

// 左上角显示的图片数据
const unsigned char logo[] PROGMEM = {0x20, 0x00, 0x00, 0x78, 0x1c, 0x00, 0x7e, 0x3e, 0x00, 0xff, 0xbf, 0x00, 0xff, 0xbe, 0x00, 0x3f, 
	0xfe, 0x00, 0x0f, 0xfe, 0x00, 0x03, 0xfe, 0x00, 0x00, 0xfd, 0x80, 0x01, 0xff, 0xe0, 0x01, 0xfb, 
	0xf8, 0x01, 0xff, 0xfc, 0x00, 0xf1, 0xfe, 0x00, 0x60, 0xfc, 0x00, 0x00, 0x3c, 0x00, 0x00, 0x08
};

// 初始化对象
TinyGPSPlus gps;
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
HardwareSerial gpsSerial(1);
MPU6050 mpu6050(Wire);

// GCJ02转换相关常量
const double a = 6378245.0;
const double ee = 0.00669342162296594323;

// 数据校验阈值
const double SPEED_THRESHOLD = 3;
const float HDOP_THRESHOLD = 2.0;
const int MIN_SATELLITES = 5;

// 全局变量
double lastValidLat = 0;
double lastValidLon = 0;
bool hasValidData = false;
bool gpsFixAcquired = false;  // 新增：GPS定位成功标志
unsigned long dataLogInterval = 5000;
unsigned long lastLogTime = 0;

// MPU6050相关变量
float accelX = 0, accelY = 0, accelZ = 0;
float gyroX = 0, gyroY = 0, gyroZ = 0;
float angleX = 0, angleY = 0;
unsigned long lastMpuUpdate = 0;

// 卡尔曼滤波参数 - 使用float提高FPU效率
struct KalmanFilter {
  float Q_angle;    // 角度过程噪声方差
  float Q_bias;     // 偏置过程噪声方差
  float R_measure;  // 测量噪声方差
  
  float angle;      // 滤波后的角度
  float bias;       // 角速度偏置
  float rate;       // 无偏角速度
  
  float P[2][2];    // 估计误差协方差矩阵
};

// 位置和速度卡尔曼滤波器实例
KalmanFilter latFilter, lonFilter, speedFilter;

// 函数声明
double transformLat(double x, double y);
double transformLon(double x, double y);
bool outOfChina(double lat, double lon);
void wgs84ToGcj02(double wgsLat, double wgsLon, double& gcjLat, double& gcjLon);
int getBeijingHour(int utcHour);
void drawLogo(int x, int y, int width, int height, const unsigned char* bitmap);
void logGPSData(double lat, double lon, double speed, int satellites);
void initKalmanFilter(KalmanFilter& kf, float qAngle, float qBias, float rMeasure);
float kalmanUpdate(KalmanFilter& kf, float measurement, float dt);
void updateMPUData();
float calculateMovementConfidence();
float calculateSpeedChangeConfidence();
bool isGPSDataReliable();
void displaySpeedInfo();

// 初始化卡尔曼滤波器
void initKalmanFilter(KalmanFilter& kf, float qAngle, float qBias, float rMeasure) {
  kf.Q_angle = qAngle;
  kf.Q_bias = qBias;
  kf.R_measure = rMeasure;
  
  kf.angle = 0.0f;
  kf.bias = 0.0f;
  kf.rate = 0.0f;
  
  kf.P[0][0] = 0.0f;
  kf.P[0][1] = 0.0f;
  kf.P[1][0] = 0.0f;
  kf.P[1][1] = 0.0f;
}

// 卡尔曼滤波更新 - 使用FPU加速
float kalmanUpdate(KalmanFilter& kf, float measurement, float dt) {
  // 预测步骤 - 使用单精度浮点提高FPU效率
  kf.rate = measurement - kf.bias;
  kf.angle += dt * kf.rate;
  
  // 更新误差协方差矩阵
  kf.P[0][0] += dt * (dt * kf.P[1][1] - kf.P[0][1] - kf.P[1][0] + kf.Q_angle);
  kf.P[0][1] -= dt * kf.P[1][1];
  kf.P[1][0] -= dt * kf.P[1][1];
  kf.P[1][1] += kf.Q_bias * dt;
  
  // 计算卡尔曼增益
  const float S = kf.P[0][0] + kf.R_measure;
  const float K0 = kf.P[0][0] / S;
  const float K1 = kf.P[1][0] / S;
  
  // 更新估计值
  const float y = measurement - kf.angle;
  kf.angle += K0 * y;
  kf.bias += K1 * y;
  
  // 更新误差协方差矩阵
  const float P00_temp = kf.P[0][0];
  const float P01_temp = kf.P[0][1];
  
  kf.P[0][0] = P00_temp - K0 * P00_temp;
  kf.P[0][1] = P01_temp - K0 * P01_temp;
  kf.P[1][0] = kf.P[1][0] - K1 * P00_temp;
  kf.P[1][1] = kf.P[1][1] - K1 * P01_temp;
  
  return kf.angle;
}

// 更新MPU6050数据
void updateMPUData() {
  mpu6050.update();
  
  accelX = mpu6050.getAccX();
  accelY = mpu6050.getAccY();
  accelZ = mpu6050.getAccZ();
  
  gyroX = mpu6050.getGyroX();
  gyroY = mpu6050.getGyroY();
  gyroZ = mpu6050.getGyroZ();
  
  angleX = mpu6050.getAngleX();
  angleY = mpu6050.getAngleY();
  
  lastMpuUpdate = millis();
}

// 计算运动置信度 (0-1)
float calculateMovementConfidence() {
  // 基于加速度计算运动强度
  float accelMagnitude = sqrtf(accelX*accelX + accelY*accelY + accelZ*accelZ);
  // 归一化到0-1范围
  float confidence = (accelMagnitude - 9.5f) / 2.0f;
  if (confidence < 0.0f) {
    return 0.0f;
  } else if (confidence > 1.0f) {
    return 1.0f;
  } else {
    return confidence;
  }
}

// 计算速度变化置信度
float calculateSpeedChangeConfidence() {
  static float lastSpeed = 0;
  static unsigned long lastTime = 0;
  
  unsigned long currentTime = millis();
  float dt = (currentTime - lastTime) / 1000.0f;
  if (dt < 0.1f) return 0.0f;  // 时间间隔太短时不计算
  
  float currentSpeed = hasValidData ? speedFilter.angle : 0;
  float speedDiff = fabsf(currentSpeed - lastSpeed);
  
  lastSpeed = currentSpeed;
  lastTime = currentTime;
  
  // 速度变化超过阈值则认为有显著变化
  if (speedDiff > 2.0f) return 1.0f;
  return 0.0f;
}

// 改进的数据有效性检查函数
bool isGPSDataReliable() {
  if (!gps.location.isValid() || !gps.hdop.isValid() || !gps.satellites.isValid()) {
    return false;
  }
  
  // 更严格的HDOP和卫星数量检查
  if (gps.hdop.hdop() > HDOP_THRESHOLD || gps.satellites.value() < MIN_SATELLITES) {
    return false;
  }
  
  // 检查速度合理性
  if (gps.speed.isValid()) {
    float speed = gps.speed.kmph();
    if (speed > 200.0f) {  // 不合理的高速
      return false;
    }
  }
  
  // 检查位置变化合理性
  if (hasValidData) {
    float distance = TinyGPSPlus::distanceBetween(
      lastValidLat, lastValidLon, 
      gps.location.lat(), gps.location.lng());
    float timeDiff = (millis() - lastMpuUpdate) / 1000.0f;
    
    if (timeDiff > 0.1f) {
      // 计算最大合理速度（300km/h）
      float maxReasonableSpeed = 300.0f / 3.6f;  // 转换为m/s
      float maxReasonableDistance = maxReasonableSpeed * timeDiff;
      
      if (distance > maxReasonableDistance) {
        return false;  // 位置变化不合理
      }
    }
  }
  
  return true;
}

int getBeijingHour(int utcHour) {
  int beijingHour = utcHour + 8;
  if (beijingHour >= 24) {
    beijingHour -= 24;
  }
  return beijingHour;
}

void drawLogo(int x, int y, int width, int height, const unsigned char* bitmap) {
  display.drawBitmap(x, y, bitmap, width, height, SSD1306_WHITE);
}

void logGPSData(double lat, double lon, double speed, int satellites) {
  if (!SPIFFS.exists("/gps_logs")) {
    SPIFFS.mkdir("/gps_logs");
  }
  
  char fileName[32];
  if (gps.date.isValid()) {
    sprintf(fileName, "/gps_logs/%04d%02d%02d.txt", 
            gps.date.year(), gps.date.month(), gps.date.day());
  } else {
    strcpy(fileName, "/gps_logs/unknown_date.txt");
  }
  
  File file = SPIFFS.open(fileName, FILE_APPEND);
  if (!file) {
    Serial.println("Failed to open log file");
    return;
  }
  
  char logLine[128];
  if (gps.time.isValid()) {
    sprintf(logLine, "[%02d:%02d:%02d] Lat:%.6f, Lon:%.6f, Speed:%.1f, Sats:%d\n",
            getBeijingHour(gps.time.hour()), gps.time.minute(), gps.time.second(),
            lat, lon, speed, satellites);
  } else {
    sprintf(logLine, "[--:--:--] Lat:%.6f, Lon:%.6f, Speed:%.1f, Sats:%d\n",
            lat, lon, speed, satellites);
  }
  
  file.print(logLine);
  file.close();
  Serial.print("Logged: ");
  Serial.print(logLine);
}

// 添加专门的速度显示函数
void displaySpeedInfo() {
  // 只有GPS定位成功后才更新速度滤波器
  if (gpsFixAcquired && gps.speed.isValid()) {
    float currentSpeed = (float)gps.speed.kmph();
    unsigned long currentTime = millis();
    float dt = (currentTime - lastMpuUpdate) / 1000.0f;
    if (dt > 0.1f) {  // 确保时间间隔合理
      speedFilter.angle = kalmanUpdate(speedFilter, currentSpeed, dt);
      lastMpuUpdate = currentTime;
    }
  }
}

void setup() {
  Serial.begin(115200);
  
  if(!SPIFFS.begin(true)){
    Serial.println("SPIFFS Mount Failed");
  }
  
  gpsSerial.begin(115200, SERIAL_8N1, GPS_RX, GPS_TX);
  gpsSerial.println("$PMTK220,100*2F");
  delay(100);
  gpsSerial.println("$PCAS11$,2*1f");
  delay(100);
  
  Wire.begin(OLED_SDA, OLED_SCL);
  
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  
  // 初始化MPU6050
  mpu6050.begin();
  mpu6050.calcGyroOffsets(true);
  
  // 初始化卡尔曼滤波器 - 修改参数
  initKalmanFilter(latFilter, 0.01f, 0.005f, 0.5f);   // 增加过程噪声，提高响应速度
  initKalmanFilter(lonFilter, 0.01f, 0.005f, 0.5f);
  initKalmanFilter(speedFilter, 0.05f, 0.01f, 1.0f);  // 速度滤波器更敏感
  
  delay(100);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print(F("Initializing..."));
  display.display();
  delay(2000);
}

void displayGPSInfo() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  drawLogo(96, 36, 23, 16, logo);
  
  // 更新MPU数据
  updateMPUData();
  
  // 计算运动置信度
  float movementConfidence = calculateMovementConfidence();
  float speedChangeConfidence = calculateSpeedChangeConfidence();
  
  bool isDataValid = gps.location.isValid() && 
                     gps.hdop.isValid() && 
                     gps.hdop.hdop() < HDOP_THRESHOLD &&
                     gps.satellites.value() >= MIN_SATELLITES &&
                     gps.location.isUpdated() &&
                     gps.time.isValid();

  // 更新GPS定位成功标志
  if (isDataValid && isGPSDataReliable()) {
    gpsFixAcquired = true;
  } else {
    // 连续3秒无效数据则认为定位丢失
    static unsigned long lostFixTime = 0;
    if (gpsFixAcquired) {
      if (lostFixTime == 0) {
        lostFixTime = millis();
      } else if (millis() - lostFixTime > 3000) {
        gpsFixAcquired = false;
        lostFixTime = 0;
      }
    }
  }

  float currentSpeed = 0.0f;
  if (gps.speed.isValid()) {
    currentSpeed = (float)gps.speed.kmph();
  }

  // 计算时间间隔
  static unsigned long lastDisplayTime = 0;
  unsigned long currentTime = millis();
  float dt = (currentTime - lastDisplayTime) / 1000.0f;
  lastDisplayTime = currentTime;

  // 动态调整滤波参数
  float dynamicQ = 0.001f + movementConfidence * 0.02f;  // 运动时增加过程噪声
  float dynamicR = 0.5f - movementConfidence * 0.4f;     // 运动时减少测量噪声

  // 更新滤波器参数
  latFilter.Q_angle = dynamicQ;
  lonFilter.Q_angle = dynamicQ;
  speedFilter.Q_angle = dynamicQ * 5.0f;  // 速度滤波器更敏感

  latFilter.R_measure = dynamicR;
  lonFilter.R_measure = dynamicR;
  speedFilter.R_measure = dynamicR * 2.0f;

  if (speedChangeConfidence > 0.5f) {
    // 速度显著变化时重置速度滤波器
    speedFilter.angle = currentSpeed;
    speedFilter.bias = 0;
  }

  // 只有GPS定位成功后才进行卡尔曼滤波
  if (gpsFixAcquired && isDataValid && isGPSDataReliable()) {
    double wgsLat = gps.location.lat();
    double wgsLon = gps.location.lng();
    double gcjLat, gcjLon;
    
    wgs84ToGcj02(wgsLat, wgsLon, gcjLat, gcjLon);
    
    // 计算数据可靠性权重
    float hdopWeight = 1.0f - (gps.hdop.hdop() / HDOP_THRESHOLD);
    float satelliteWeight = (float)gps.satellites.value() / 12.0f;
    if (satelliteWeight > 1.0f) satelliteWeight = 1.0f;
    
    float dataReliability = hdopWeight * satelliteWeight;
    
    float filteredLat, filteredLon, filteredSpeed;
    
    // 应用加权卡尔曼滤波
    if (hasValidData) {
      // 仅在GPS定位成功后执行卡尔曼滤波更新
      filteredLat = kalmanUpdate(latFilter, (float)gcjLat, dt) * (1.0f - dataReliability) + 
                   (float)gcjLat * dataReliability;
      filteredLon = kalmanUpdate(lonFilter, (float)gcjLon, dt) * (1.0f - dataReliability) + 
                   (float)gcjLon * dataReliability;
      filteredSpeed = kalmanUpdate(speedFilter, currentSpeed, dt) * (1.0f - dataReliability) + 
                     currentSpeed * dataReliability;
      
      // 当检测到运动时，增加GPS权重
      if (movementConfidence > 0.3f) {
        // 动态调整滤波结果，运动时更信任GPS
        filteredLat = filteredLat * (1.0f - movementConfidence) + (float)gcjLat * movementConfidence;
        filteredLon = filteredLon * (1.0f - movementConfidence) + (float)gcjLon * movementConfidence;
        filteredSpeed = filteredSpeed * (1.0f - movementConfidence) + currentSpeed * movementConfidence;
      }
    } else {
      // 首次获取有效数据
      filteredLat = (float)gcjLat;
      filteredLon = (float)gcjLon;
      filteredSpeed = currentSpeed;
      
      latFilter.angle = filteredLat;
      lonFilter.angle = filteredLon;
      speedFilter.angle = filteredSpeed;
      
      hasValidData = true;
    }
    
    // 更新历史有效数据
    lastValidLat = filteredLat;
    lastValidLon = filteredLon;
    
    // 显示滤波后的经纬度
    display.print(F("Lat: "));
    display.print(filteredLat, 6);
    display.print(F(" "));
    display.print(filteredLat > 0 ? F("N") : F("S"));
    
    display.setCursor(0, 12);
    display.print(F("Lng: "));
    display.print(filteredLon, 6);
    display.print(F(" "));
    display.print(filteredLon > 0 ? F("E") : F("W"));
    
    // 定时记录数据
    if(millis() - lastLogTime >= dataLogInterval) {
      lastLogTime = millis();
      logGPSData(filteredLat, filteredLon, filteredSpeed, gps.satellites.value());
    }
  } else {
    // 无效数据时使用上次有效数据
    if (hasValidData) {
      display.print(F("Using filtered data"));
      display.setCursor(0, 12);
      display.print(F("Lat: "));
      display.print(lastValidLat, 6);
      display.setCursor(0, 24);
      display.print(F("Lng: "));
      display.print(lastValidLon, 6);
    } else {
      display.println(F("No valid data"));
      if (gps.hdop.isValid()) {
        display.print(F("HDOP: "));
        display.println(gps.hdop.hdop());
      }
      display.print(F("Sats: "));
      display.println(gps.satellites.value());
    }
    
    // 显示GPS状态
    display.setCursor(0, 34);
    display.print(F("GPS: "));
    display.print(gpsFixAcquired ? F("Fix Lost") : F("No Fix"));
  }
  
  // 显示卫星数量
  display.setCursor(SCREEN_WIDTH - 24 , 22);
  display.print(gps.satellites.value());
  
  // 显示速度信息
  display.setCursor(0 , 22);
  float displaySpeed = hasValidData ? speedFilter.angle : currentSpeed;
  if (displaySpeed < SPEED_THRESHOLD) {
    display.print(F("Spd: 0.0km/h"));
  } else {
    display.print(F("Spd: "));
    display.print(displaySpeed, 1);
    display.print(F("km/h"));
  }
  
  // 显示运动状态
  display.setCursor(0, 46);
  if (movementConfidence < 0.1f) {
    display.print(F("Status: Stationary"));
  } else if (movementConfidence < 0.5f) {
    display.print(F("Status: Moving slow"));
  } else {
    display.print(F("Status: Moving fast"));
  }
  
  // 显示时间（北京时间）
  if (gps.time.isValid()) {
    display.setCursor(0, 34);
    display.print(F("Time: "));
    display.print(getBeijingHour(gps.time.hour()));
    display.print(F(":"));
    if (gps.time.minute() < 10) display.print(F("0"));
    display.print(gps.time.minute());
    display.print(F(":"));
    if (gps.time.second() < 10) display.print(F("0"));
    display.print(gps.time.second());
  }
  
  display.display();
}

void loop() {
  while (gpsSerial.available() > 0) {
    char c = gpsSerial.read();
    gps.encode(c);
    
    // 特别处理速度相关句子
    if (c == '$') {
      // 检查是否是速度相关句子（如RMC）
      String sentence = gpsSerial.readStringUntil('\n');
      if (sentence.startsWith("GPRMC") || sentence.startsWith("GNRMC")) {
        // 强制更新速度显示
        displaySpeedInfo();
      }
    }
  }
  
  // 原有的显示更新逻辑
  static unsigned long lastUpdate = 0;
  if (millis() - lastUpdate >= 1000) {  // 缩短更新间隔到1秒
    lastUpdate = millis();
    displayGPSInfo();
  }
  
  if (millis() > 5000 && gps.charsProcessed() < 10) {
    Serial.println(F("No GPS data received: check wiring"));
  }
}

// GCJ02坐标转换实现
double transformLat(double x, double y) {
  double ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * sqrt(fabs(x));
  ret += (20.0 * sin(6.0 * x * PI) + 20.0 * sin(2.0 * x * PI)) * 2.0 / 3.0;
  ret += (20.0 * sin(y * PI) + 40.0 * sin(y / 3.0 * PI)) * 2.0 / 3.0;
  ret += (160.0 * sin(y / 12.0 * PI) + 320 * sin(y * PI / 30.0)) * 2.0 / 3.0;
  return ret;
}

double transformLon(double x, double y) {
  double ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * sqrt(fabs(x));
  ret += (20.0 * sin(6.0 * x * PI) + 20.0 * sin(2.0 * x * PI)) * 2.0 / 3.0;
  ret += (20.0 * sin(x * PI) + 40.0 * sin(x / 3.0 * PI)) * 2.0 / 3.0;
  ret += (150.0 * sin(x / 12.0 * PI) + 300.0 * sin(x / 30.0 * PI)) * 2.0 / 3.0;
  return ret;
}

bool outOfChina(double lat, double lon) {
  if (lon < 72.004 || lon > 137.8347)
    return true;
  if (lat < 0.8293 || lat > 55.8271)
    return true;
  return false;
}

void wgs84ToGcj02(double wgsLat, double wgsLon, double& gcjLat, double& gcjLon) {
  if (outOfChina(wgsLat, wgsLon)) {
    gcjLat = wgsLat;
    gcjLon = wgsLon;
    return;
  }
  
  double dLat = transformLat(wgsLon - 105.0, wgsLat - 35.0);
  double dLon = transformLon(wgsLon - 105.0, wgsLat - 35.0);
  double radLat = wgsLat / 180.0 * PI;
  double magic = sin(radLat);
  magic = 1 - ee * magic * magic;
  double sqrtMagic = sqrt(magic);
  
  dLat = (dLat * 180.0) / ((a * (1 - ee)) / (magic * sqrtMagic) * PI);
  dLon = (dLon * 180.0) / (a / sqrtMagic * cos(radLat) * PI);
  
  gcjLat = wgsLat + dLat;
  gcjLon = wgsLon + dLon;
}